%% Skrypt identyfikujacy system i estymujacy kolejny stan systemu
% Parametry:
%   ucsv    -   nazwa pliku z macierza czasowa wektora sterowania (kolejne
%                   kolumny to kolejne wektory dla kolejnych chwil)
%   ycsv    -   nazwa pliku z macierza czasowa wektora wyjscia (kolejne
%                   kolumny to kolejne wektory dla kolejnych chwil)
%   Ts      -   czas probkowania
%   delay   -   opoznienie systemu
%   ocsv    -   nazwa pliku, do ktorego zapisujemy wyniki

function estymuj(ucsv,ycsv,Ts,delay,ocsv)

display(' ');
display('Rozpoczecie estymacji...');

%% wczytanie danych

u = csvread(ucsv);
y = csvread(ycsv);
u=u';
y=y';

ny = length(y(1,:));
nu = length(u(1,:));
na = eye(ny);
nk = ones(ny,nu)*delay;
nb = zeros(ny,nu);
nb(ny,nu) = 1;

%% identyfikacja

sys = identyfikuj(u,y,Ts,na,nb,nk);

%% przygotowanie danych dla kalmana

system.x = nan;
system.y = y(end,:)';
system.u = u(end,:)';
system.A = sys.a;
system.B = sys.b;
system.C = sys.c;%lepiej by było zamienic na diag(1)
%system.C = diag(diag(eye(length(system.A(:,1)))));
system.P = nan;
system.Q = 0;
system.R = cov(y);

%% inicjalizacja kalmana

system = kalmanuj(system);

%% estymacja kalmanem

system = kalmanuj(system);

%% zapis danych

csvwrite(ocsv,system.x');

display('Estymacja zakonczona.');
display(' ');